﻿#include "hcHeadFile.h"
#include <vector>

using namespace std;

/*
data_2d 【输入】  2d图片的数据首地址，单通道灰度图
data_3d 【输入】  点云的数据首地址，三通道，每个通道分别保存x，y，z的数据
preload_data 【输入】  预加载的数据，可以为空。方便优先数据预加载，比如深度学习模型，每次检测时就不需要每次加载模型而浪费时间了
result 【输出】  存放位姿结果
注意：函数前需要加上  extern "C"  _declspec(dllexport)  不然无法正常读取dll
*/
extern "C"  _declspec(dllexport) int myFunction(unsigned char* data_2d, float* data_3d, const PreloadData& preload_data, HcDetectorResultData& result)
{	
	//...
	//...
	//经过复杂的识别算法，假设识别出2个姿态结果 pose1 pose2（假设用旋转矩阵表示）
	float pose1[16] = {
		-0.999938, 0.00919979, 0.00627937, 0,
		0.00902145, 0.999571, -0.0278624,  0,
		- 0.006533, -0.0278041, -0.999592, 0,
		151.555,	-775.288,	67.25,     1};
	float pose2[16] = {
		-0.134271,  0.989123,   0.060051, 0,
		 0.990844,  0.134874,  -0.006076, 0,
		-0.0141092, 0.0586855, -0.998177, 0,
		-37.261,	-1891.93,	2690.39,  1 };

	vector<float*> all_pose = { pose1 , pose2 };

	for (int i = 0; i < all_pose.size(); i++)
	{
		unsigned int current_count = result.detectedPoses.size;
		result.detectedPoses.poses[current_count].poseId = current_count;
		//将位姿对应的旋转矩阵拷贝进结果
		memcpy(result.detectedPoses.poses[current_count].rtMtrix, all_pose[i], 16 * sizeof(float));
		result.poseScores.scores[current_count] = 0.9;//分数
		result.detectedPoses.size++;
		result.poseScores.size++;	
	}

	return all_pose.size();
}

//
int main(int argc, char*argv[])
{
	

	return 0;
}
